/**
|--------------------------------Copyright-------------------------------------|
|                                                                              |
|                      (C)Copyright 2019,Chen Yikuang                          |
|                                                                              |
|------------------------------------------------------------------------------|
|  FileName     : Chassis.c
|  Version      : v1.0
|  Author       : Chen Yikuang
|  Date         : 2019-05-23
|  Libsupports  : STM32F10x_StdPeriph_Lib_V3.5.0
|  Description  :
|------------------------------declaration of end------------------------------|
 **/
/*------------------------ I N C L U D E - F I L E S -------------------------*/
#include "bsp_encoder.h"
#include "Chassis.h"
#include "Users.h"

/*----------------------------- D E F I N E S --------------------------------*/
/*------------ L O C A L - F U N C T I O N S - P R O T O T Y P E S -----------*/
Chassis_t Chassis;
int32_t MotorSpeed[1];
/*--------------------- G L O B A L - F U N C T I O N S ----------------------*/
/**/
/*----------------------- L O C A L - F U N C T I O N S ----------------------*/
/**/
/*------------------------------- FILE OF END --------------------------------*/


/**
  * @Data   2019-05-23 12:46
  * @brief  底盘进程
  * @param  void
  * @retval void
  */
void Chassis_processing(void)
{
	uint8_t i;

	for(i=0;i<Chassis_MotorNum;i++)
	{
		switch (i)
		{
			case One:
				abs_limit_f(&(Motor[One].MotorPID.Target),(float)targetSpeedMax_79Y);
				MotorSpeed[i]=Incremental_PID(&Motor[One].MotorPID,
																		Motor[One].MotorPID.Target,
																		Motor[One].MotorPID.Measured);
				break;
		}
	}
	
	for(i=0;i<Chassis_MotorNum;i++){
		Motor_setSpeed(&Motor[i],MotorSpeed[i]);
	}
	
}


/**
  * @Data   2019-03-23 12:46
  * @brief  底盘运动模型解算
  * @param  void
  * @retval void
  */
void Chassis_Algorithm_Model(void)
{
#ifdef Chasis_FourOmniWheel
	//   +x向前
	//   +y向左
	//   左转正，右转负
	Motor[One].MotorPID.Target   =  Chassis.Vx + Chassis.Vy + Chassis.Vspin;
	Motor[Two].MotorPID.Target   = -Chassis.Vx + Chassis.Vy - Chassis.Vspin;
	Motor[Three].MotorPID.Target =  Chassis.Vx + Chassis.Vy - Chassis.Vspin;
	Motor[Four].MotorPID.Target  = -Chassis.Vx + Chassis.Vy + Chassis.Vspin;
#endif
}

